#include<stdio.h>
#include<string.h>
#include <Dense>
#include<SVD>
#include<iostream>

using namespace Eigen;

#ifndef SIGN
#define SIGN(x) ((x>0)?1:((x<0)?-1:0))
#endif SIGN

#include<Mesh/libcell_iterator.h>

#include<Mesh_IO/Mesh_IO.h>
#include<libcell_macros.h>

#define quote lib_cell_quote
int smallest_bounding_box(double**points,int n, double*trans ,double*length,double* width,double*height )
{
    if(points==NULL||n<=0)
    {
        return 0;
    }
    Eigen::MatrixXd A;
    A.resize(n,3);
    A.setZero();
    for(int i=0;i<n;i++)
    {
        for(int j=0;j<3;j++)
        {
            A(i,j)=points[i][j];
        }
    }
    Eigen::MatrixXd B=(MatrixXd::Ones(1,n)*A)*((1.0/(double)n));
    for(int i=0;i<n;i++)
    {
        A.row(i)=A.row(i)-B;
    }
    JacobiSVD<Eigen::MatrixXd> svd(A,ComputeThinU|ComputeThinV);
    MatrixXd V=svd.matrixV();

    memset(trans,0,sizeof(double)*16);
    for(int i=0;i<3;i++)
    {
        for(int j=0;j<3;j++)
        {
            trans[j*4+i]=V(i,j);
        }
    }   
    trans[12]=B(0);trans[13]=B(1);trans[14]=B(2);
    trans[15]=1.0;
    //memmove(trans,V.data(),sizeof(double)*9); 
    MatrixXd C=V.inverse();
    Vector3d X,Y;
    double xs[2]={0,0},ys[2]={0,0},zs[2]={0,0};
    X(0)=points[0][0];X(1)=points[0][1];X(2)=points[0][2];
    Y=C*X;
    xs[0]=Y(0); xs[1]=Y(0);   
    ys[0]=Y(1); ys[1]=Y(1);   
    zs[0]=Y(2); zs[1]=Y(2);

    for(int i=0;i<n;i++)
    {
        for(int j=0;j<3;j++)
        {
            X(j)=points[i][j];
        }
        Y=C*X;
        Y(0)>xs[1]?(xs[1]=Y(0)):(Y(0)<xs[0]?(xs[0]=Y(0)):(xs[0]=xs[0]));
        Y(1)>ys[1]?(ys[1]=Y(1)):(Y(1)<ys[0]?(ys[0]=Y(1)):(ys[0]=ys[0]));
        Y(2)>zs[1]?(zs[1]=Y(2)):(Y(2)<zs[0]?(zs[0]=Y(2)):(zs[0]=zs[0]));
    }
    *length=xs[1]-xs[0];
    *width=ys[1]-ys[0];
    *height=zs[1]-zs[0];
    return 1;
}

int main()
{
    double va[9]={0,1,2,3,4,5,6,7,8};
    Eigen::MatrixXd A;
    A.resize(3,3);
    A.setZero();
    memmove(A.data(),va,sizeof(double)*9);

    Mesh mesh;
    Mesh_init(&mesh);
    _ReadOff_(&mesh,"4Boundary.off" ,3);
    printf("%d\n",mesh.num_v(&mesh));
    double** points=(double**)malloc(sizeof(double*)*mesh.num_v(&mesh));
    int i=0;
    for(auto it=mesh.v_begin(&mesh);it!=mesh.v_end(&mesh);it++)
    {
        points[i]=(double*)malloc(sizeof(double)*3);
        points[i][0]=quote(it)->point[0]; points[i][1]=quote(it)->point[1]; points[i][2]=quote(it)->point[2];  

        i++;
    }

    double trans[16]={0} ; 
    double  length,width,height;
    smallest_bounding_box(points,mesh.num_v(&mesh),trans,&length,&width,&height); 
    //printf("%lf %lf %lf\n",barycentre[0],barycentre[1],barycentre[2]);
    printf("%lf %lf %lf \n",length,width,height);
    Mesh_free(&mesh);

    std::cout<<A<<std::endl;

    return 0;
}


